/**
 * @file ebex_statistics.c
 *
 * @date Mar 3, 2010
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include <ebex_sort.h>
#include <ebex_util.h>
#include <memory_util.h>
#include <log_util.h>
#include <ebex_statistics.h>

typedef double (*ebex_stats_residual_fn)(double, double);

double ebex_stats_mean(const size_t m_dim, const double *m_data)
{
	double result = 0.;
	int i;
	for (i = 0; i < m_dim; i++)
		result += m_data[i];
	result /= m_dim;
	return result;
}

/**
 * Computs the media of a data vector by using as much of a
 * quicksort algorithm as needed to isolate the median.  On
 * return, #m_data is partially sorted.
 * @param m_dim Number of elements in m_data
 * @param m_data Double vector of data elements from which to find the median
 * @return Median of data
 */
double ebex_stats_median(size_t m_dim, double *m_data)
{
	size_t i, j;
	size_t nr = m_dim / 2;
	size_t nl = nr - 1;
	size_t even = 0;
	/* hi & lo are position limits encompassing the median. */
	int lo = 0;
	int hi = m_dim - 1;

	if (m_dim == 2 * nr)
		even = 1;
	if (m_dim < 3)
	{
		if (m_dim < 1)
			return 0.;
		if (m_dim == 1)
			return m_data[0];
		return 0.5 * (m_data[0] + m_data[1]);
	}

	/* Find median of 1st, middle & last values. */
	do
	{
		int loop;
		int mid = (lo + hi) / 2;
		double result = m_data[mid];
		double xlo = m_data[lo];
		double xhi = m_data[hi];
		if (xhi < xlo)
		{
			double temp = xlo;
			xlo = xhi;
			xhi = temp;
		}
		if (result > xhi)
			result = xhi;
		else if (result < xlo)
			result = xlo;
		/* The basic quicksort algorithm to move all values <= the sort key (XMED)
		 * to the left-hand end, and all higher values to the other end.
		 */
		i = lo;
		j = hi;
		do
		{
			while (m_data[i] < result)
				i++;
			while (m_data[j] > result)
				j--;
			loop = 0;
			if (i < j)
			{
				double temp = m_data[i];
				m_data[i] = m_data[j];
				m_data[j] = temp;
				i++;
				j--;
				if (i <= j)
					loop = 1;
			}
		} while (loop); /* Decide which half the median is in. */

		if (even)
		{
			if (j == nl && i == nr)
			/* Special case, n even, j = n/2 & i = j + 1, so the median is
			 * between the two halves of the series.   Find max. of the first
			 * half & min. of the second half, then average.
			 */
			{
				int k;
				double xmax = m_data[0];
				double xmin = m_data[m_dim - 1];
				for (k = lo; k <= j; k++)
					xmax = MAX(xmax, m_data[k]);
				for (k = i; k <= hi; k++)
					xmin = MAX(xmin, m_data[k]);
				return 0.5 * (xmin + xmax);
			}
			if (j < nl)
				lo = i;
			if (i > nr)
				hi = j;
			if (i == j)
			{
				if (i == nl)
					lo = nl;
				if (j == nr)
					hi = nr;
			}
		}
		else
		{
			if (j < nr)
				lo = i;
			if (i > nr)
				hi = j;
			/* Test whether median has been isolated. */
			if (i == j && i == nr)
				return result;
		}
	} while (lo < hi - 1);

	if (even)
		return (0.5 * (m_data[nl] + m_data[nr]));
	if (m_data[lo] > m_data[hi])
	{
		double temp = m_data[lo];
		m_data[lo] = m_data[hi];
		m_data[hi] = temp;
	}
	return m_data[nr];
}

/**
 * Calculates the variance of a data vector.  Based on a method described in
 * "Note on a method for calculating corrected sums of squares and products."
 * Technometrics 4(3): 419-420 (1962).
 * @param m_numdata Number of data elements
 * @param m_data Data vector
 * @param m_datastride Stride between consecutive data elements
 * @param m_biased If false, calculated an unbiased estimate
 * @return Variance of data vector
 */
double ebex_stats_dbl_mean_variance(size_t m_numdata, const double *m_data, size_t m_datastride, bool m_biased, double *m_mean)
{
	size_t i;
	double mean = 0.0;
	double result = 0.0;
	double value;
	double delta;
	const double* p = m_data;
	for (i = 1; i <= m_numdata; i++, p += m_datastride)
	{
		value = *p;
		delta = value - mean;
		mean += delta / (double)i;
		result += delta * (value - mean);
	}
	if (!m_biased)
		result /= (m_numdata - 1); /* Unbiased estimate */
	else
		result /= m_numdata; /* Maximum-likelihood estimate */

	if (m_mean) *m_mean = mean;
	return result;
}

double ebex_stats_dbl_weighted_delta_variance(size_t m_numdata, const double *m_data, const double *m_weight, bool m_biased)
{
	double denominator = 1.0;
	double variance = 0.0;
	double mean = 0.0;
	if (!m_biased)
	{
		denominator = 0.0;
		for (size_t i = 0; i < m_numdata; i++)
			denominator += pow(m_weight[i], 2.0);

		denominator = 1.0 - denominator;
	}

	mean = ebex_kahan_sum(m_numdata, m_data, 1) / (double) m_numdata;

	for (size_t i = 0; i < m_numdata; i++)
	{
		variance += m_weight[i] * pow(m_data[i] - mean, 2.0);
	}

	return variance / denominator;
}

int ebex_stats_covariance(int m_num, int m_dim, const double *m_data, size_t m_major_stride, size_t m_minor_stride, bool m_biased, double *m_cov)
{
	int i, j, k;
	double *output1;
	double *output2;
	const double *q;
	const double *q1;
	const double *q2;
	double x1;
	double x2;
	double denominator = (double)(m_biased ? m_num : (m_num - 1));
	double average[m_dim];

	/* Initialize to zero */
	output1 = m_cov;
	for (i = 0; i < m_dim; i++)
	{
		average[i] = 0.0;
		for (j = 0; j < m_dim; j++, output1++)
			*output1 = 0.0;
	}

	/* Calculate the sums of squares */
	for (k = 0; k < m_num; k++)
	{
		const double scale = k + 1.0;
		const double factor = k / scale;
		q = m_data + k * m_major_stride;
		q1 = q;
		for (i = 0; i < m_dim; i++, q1 += m_minor_stride)
		{
			output1 = m_cov + i * m_dim;
			x1 = (*q1) - average[i];
			q2 = q;
			for (j = 0; j <= i; j++, output1++, q2 += m_minor_stride)
			{
				x2 = (*q2) - average[j];
				*output1 += factor * x1 * x2;
			}
		}
		for (i = 0; i < m_dim; i++, q += m_minor_stride)
			average[i] = factor * average[i] + (*q) / scale;
	}

	/* Scale, and copy to the upper half of the matrix */
	for (i = 0; i < m_dim; i++)
	{
		output1 = m_cov + i * m_dim;
		output2 = m_cov + i + (i + 1) * m_dim;
		output1[i] /= denominator;
		for (j = i + 1; j < m_dim; j++, output2 += m_dim)
		{
			(*output2) /= denominator;
			output1[j] = (*output2);
		}
	}
	return 1;
}

void ebex_stats_weighted_covariance(int m_num, int m_dim, const double *m_data, const double *m_mean, const double *m_weight,
                                   size_t m_major_stride, size_t m_minor_stride, bool m_biased, double *m_cov)
{
	int i, j, k;
	double *output1;
	double *output2;
	const double *q;
	const double *q1;
	const double *q2;
	double x1;
	double x2;

	double sum_weights = 0.0;
	double sum_sq_weights = 0.0;

	/* Initialize to zero */
	output1 = m_cov;
	for (i = 0; i < m_dim; i++)
	{
		for (j = 0; j < m_dim; j++, output1++)
			*output1 = 0.0;
	}

	/* Calculate the sums of squares */
	for (k = 0; k < m_num; k++)
	{
		q = m_data + k * m_major_stride;
		q1 = q;
		for (i = 0; i < m_dim; i++, q1 += m_minor_stride)
		{
			output1 = m_cov + i * m_dim;
			x1 = (*q1) - m_mean[i];
			q2 = q;
			for (j = 0; j <= i; j++, output1++, q2 += m_minor_stride)
			{
				x2 = (*q2) - m_mean[j];
				*output1 += m_weight[k] * x1 * x2;
			}
		}
		sum_weights += m_weight[k];
		sum_sq_weights += (m_weight[k] * m_weight[k]);
	}

	if (m_biased)
	{
		x1 = sum_weights / (pow(sum_weights, 2.0) - sum_sq_weights);
	}
	else
	{
		x1 = sum_weights / pow(sum_weights, 2.0);
	}
	/* Scale, and copy to the upper half of the matrix */
	for (i = 0; i < m_dim; i++)
	{
		output1 = m_cov + i * m_dim;
		output2 = m_cov + i + (i + 1) * m_dim;
		output1[i] *= x1;
		for (j = i + 1; j < m_dim; j++, output2 += m_dim)
		{
			(*output2) *= x1;
			output1[j] = (*output2);
		}
	}
}

void ebex_stats_pdf(int ndata, double data[], int dstride, double weight[], int wstride, double m_bandwidth,
		e_kernel_type m_kernel, int m_outputnum, double *m_outputx,
		int m_outputx_stride, double *m_outputy)
{
	int i, j;
	double factor;
	ebex_stats_residual_fn residual_fn;

	if (weight)
	{
		factor = 0.0;
		for (j = 0; j < ndata; j++)
			factor += weight[j];
		factor = 1.0 / (factor * m_bandwidth);
	}
	else
	{
		factor = 1.0 / (ndata * m_bandwidth);
	}
	switch (m_kernel)
	{
		case kernel_type_epanechnikov:
		{
			factor *= 0.75;
			residual_fn = ebex_stats_residual_epanechnikov;
			break;
		}
		case kernel_type_uniform:
		{
			residual_fn = ebex_stats_residual_uniform;
			break;
		}
		case kernel_type_triangle:
		{
			residual_fn = ebex_stats_residual_triangle;
			break;
		}
		case kernel_type_gaussian:
		{
			factor *= 0.5 * M_SQRT1_2 * M_2_SQRTPI;
			residual_fn = ebex_stats_residual_gaussian;
			break;
		}
		case kernel_type_quartic:
		{
			factor *= 0.9375;
			residual_fn = ebex_stats_residual_quartic;
			break;
		}
		case kernel_type_triweight:
		{
			factor *= 1.09375;
			residual_fn = ebex_stats_residual_triweight;
			break;
		}
		case kernel_type_cosine:
		{
			factor *= M_PI_4;
			residual_fn = ebex_stats_residual_cosine;
			break;
		}
	}


	if (weight)
	{
		for (i = 0; i < m_outputnum; i++)
		{
			double temp = m_outputx[i * m_outputx_stride];
			m_outputy[i] = 0.0;
			for (j = 0; j < ndata; j++)
			{
				double u = (data[j * dstride] - temp) / m_bandwidth;
				if (u < -m_bandwidth || u > m_bandwidth)
					continue;
				m_outputy[i] += residual_fn(u, m_bandwidth) * weight[j * wstride];
			}
		}
	}
	else
	{
		for (i = 0; i < m_outputnum; i++)
		{
			double temp = m_outputx[i * m_outputx_stride];
			m_outputy[i] = 0.0;
			for (j = 0; j < ndata; j++)
			{
				double u = (data[j * dstride] - temp) / m_bandwidth;
				if (u < -m_bandwidth || u > m_bandwidth)
					continue;
				m_outputy[i] += residual_fn(u, m_bandwidth);
			}
		}
	}

	for (i = 0; i < m_outputnum; i++)
		m_outputy[i] *= factor;
}

/* ********************************************************************* */

void ebex_stats_cpdf(int m_numdata, const double *m_data, int m_datastride, double m_bandwidth, e_kernel_type m_kernel,
		int m_outputnum, double *m_outputx, int m_outputx_stride, double *m_outputy)
{
	int i, j;
	switch (m_kernel)
	{
		case kernel_type_epanechnikov:
		{
			const double factor = 0.25 / m_numdata;
			for (i = 0; i < m_outputnum; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					if (u < - 1.0)
						continue;
					if (u >  1.0)
						m_outputy[i] += 4.0;
					else
						m_outputy[i] += (-u * u * u + 3 * u + 2);
				}
			}
			for (i = 0; i < m_outputnum; i++)
				m_outputy[i] *= factor;
			return;
		}
		case kernel_type_uniform:
		{
			const double factor = 0.5 / m_numdata;
			for (i = 0; i < m_outputnum; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					if (u < - 1.0)
						continue;
					if (u >  1.0)
						m_outputy[i] += 2.0;
					else
						m_outputy[i] += u + 1.0;
				}
			}
			for (i = 0; i < m_outputnum; i++)
				m_outputy[i] *= factor;
			return;
		}
		case kernel_type_triangle:
		{
			for (i = 0; i < m_outputnum; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					if (u < - 1.0)
						continue;
					else if (u < 0.0)
						m_outputy[i] += 0.5 * u * u + u + 0.5;
					else if (u < 1.0)
						m_outputy[i] += -0.5 * u * u + u + 0.5;
					else
						m_outputy[i] += 1.0;
				}
			}
			for (i = 0; i < m_outputnum; i++)
				m_outputy[i] /= m_numdata;
			return;
		}
		case kernel_type_gaussian:
		{
			const double factor = 0.5 / m_numdata;
			for (i = 0; i < m_outputnum; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					m_outputy[i] += erf(u * M_SQRT1_2);
				}
			}
			for (i = 0; i < m_outputnum; i++)
				m_outputy[i] = m_outputy[i] * factor + 0.5;
			return;
		}
		case kernel_type_quartic:
		{
			const double factor = 0.0625 / m_numdata;
			for (i = 0; i < m_outputnum; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					if (u < - 1.0)
						continue;
					if (u >  1.0)
						m_outputy[i] += 16.0;
					else
						m_outputy[i] += (8.0
								+ 15.0 * u
								- 10.0 * u * u * u
								+  3.0 * u * u * u * u * u);
				}
			}
			for (i = 0; i < m_outputnum; i++)
				m_outputy[i] *= factor;
			return;
		}
		case kernel_type_triweight:
		{
			const double factor = 0.03125 / m_numdata;
			for (i = 0; i < m_outputnum; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					if (u < - 1.0)
						continue;
					if (u >  1.0)
						m_outputy[i] += 32.0;
					else
						m_outputy[i] += (16.0
								+ 35.0 * u
								- 35.0 * u * u * u
								+ 21.0 * u * u * u * u * u
								-  5.0 * u * u * u * u * u * u * u);
				}
			}
			for (i = 0; i < m_outputnum; i++)
				m_outputy[i] *= factor;
			return;
		}
		case kernel_type_cosine:
		{
			const double factor = 0.5 / m_numdata;
			for (i = 0; i < m_outputnum; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					if (u < - 1.0)
						continue;
					if (u >  1.0)
						m_outputy[i] += 2.0;
					else
						m_outputy[i] += sin(M_PI_2 * u) + 1.0;
				}
			}
			for (i = 0; i < m_outputnum; i++)
				m_outputy[i] *= factor;
			return;
		}
	}
}

/**
 * Returns the complementary cumulative probability distribution
 * @param m_numinputdata
 * @param m_data
 * @param m_datastride
 * @param m_bandwidth
 * @param m_kernel
 * @param m_numoutputdata
 * @param m_outputx
 * @param m_outputx_stride
 * @param m_outputy
 */
void ebex_stats_cpdfc(int m_numinputdata, double *m_data, int m_datastride,
		double m_bandwidth, e_kernel_type m_kernel, int m_numoutputdata,
		double m_outputx[], int m_outputx_stride, double *m_outputy)
{
	int i, j;
	switch (m_kernel)
	{
		case kernel_type_epanechnikov:
		{
			const double factor = 0.25 / m_numinputdata;
			for (i = 0; i < m_numoutputdata; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numinputdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					if (u >  1.0)
						continue;
					if (u < - 1.0)
						m_outputy[i] += 4.0;
					else
						m_outputy[i] += (2 + u * u * u - 3 * u);
				}
			}
			for (i = 0; i < m_numoutputdata; i++)
				m_outputy[i] *= factor;
			return;
		}
		case kernel_type_uniform:
		{
			const double factor = 0.5 / m_numinputdata;
			for (i = 0; i < m_numoutputdata; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numinputdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					if (u >  1.0)
						continue;
					if (u < - 1.0)
						m_outputy[i] += 2.0;
					else
						m_outputy[i] += 1.0 - u;
				}
			}
			for (i = 0; i < m_numoutputdata; i++)
				m_outputy[i] *= factor;
			return;
		}
		case kernel_type_triangle:
		{
			for (i = 0; i < m_numoutputdata; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numinputdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					if (u >  1.0)
						continue;
					else if (u > 0.0)
						m_outputy[i] += +0.5 * u * u - u + 0.5;
					else if (u > -1.0)
						m_outputy[i] += -0.5 * u * u - u + 0.5;
					else
						m_outputy[i] += 1.0;
				}
			}
			for (i = 0; i < m_numoutputdata; i++)
				m_outputy[i] /= m_numinputdata;
			return;
		}
		case kernel_type_gaussian:
		{
			const double factor = 0.5 / m_numinputdata;
			for (i = 0; i < m_numoutputdata; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numinputdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					m_outputy[i] += erfc(u * M_SQRT1_2);
				}
			}
			for (i = 0; i < m_numoutputdata; i++)
				m_outputy[i] = m_outputy[i] * factor;
			return;
		}
		case kernel_type_quartic:
		{
			const double factor = 0.0625 / m_numinputdata;
			for (i = 0; i < m_numoutputdata; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numinputdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					if (u >  1.0)
						continue;
					if (u < - 1.0)
						m_outputy[i] += 16.0;
					else
						m_outputy[i] += (8.0 - 15.0 * u + 10.0 * u * u * u - 3.0 * u * u * u * u * u);
				}
			}
			for (i = 0; i < m_numoutputdata; i++)
				m_outputy[i] *= factor;
			return;
		}
		case kernel_type_triweight:
		{
			const double factor = 0.03125 / m_numinputdata;
			for (i = 0; i < m_numoutputdata; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numinputdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					if (u >  1.0)
						continue;
					if (u < - 1.0)
						m_outputy[i] += 32.0;
					else
						m_outputy[i] += 16.0 - 35.0 * u + 35.0 * u * u * u - 21.0 * u * u * u * u * u
								+ 5.0 * u * u * u * u * u * u * u;
				}
			}
			for (i = 0; i < m_numoutputdata; i++)
				m_outputy[i] *= factor;
			return;
		}
		case kernel_type_cosine:
		{
			const double factor = 0.5 / m_numinputdata;
			for (i = 0; i < m_numoutputdata; i++)
			{
				double ordinate = m_outputx[i * m_outputx_stride];
				m_outputy[i] = 0.0;
				for (j = 0; j < m_numinputdata; j++)
				{
					double u = (ordinate - m_data[j * m_datastride]) / m_bandwidth;
					if (u >  1.0)
						continue;
					if (u < - 1.0)
						m_outputy[i] += 2.0;
					else
						m_outputy[i] += 1.0 - sin(M_PI_2 * u);
				}
			}
			for (i = 0; i < m_numoutputdata; i++)
				m_outputy[i] *= factor;
			return;
		}
	}
}

/**
 * Smoothes a data stream by convolving with a kernel that is fit naively to the
 * local data.
 * @param m_numinput Number of input data elements total
 * @param m_inputy Vector of input y data (must be #m_numinput * #m_inputy_stride in length)
 * @param m_inputy_stride Stride to next input y value
 * @param m_inputx Vector of input x data (must be #m_numinput * #m_inputx_stride in length)
 * @param m_inputx_stride Stride to next input x value
 * @param m_bandwidth Width (in #m_inputx units) between median value and extrema
 * @param m_kernel Type of kernel to use
 * @param m_numoutput Number of output data points
 * @param m_outputx Vector (#m_numoutput in size) containing the new x values
 * @param m_outputx_stride Stride to next x data value
 * @param m_outputy Vector (#m_numoutput in size) to contain the calculated y values
 * @param m_outputy_stride Stride to next y output value
 */
void ebex_stats_smooth(
		size_t m_numinput,
		double* m_inputy, size_t m_inputy_stride,
		double *m_inputx, size_t m_inputx_stride,
		double m_bandwidth, e_kernel_type m_kernel,
		size_t m_numoutput,
		double *m_outputx, size_t m_outputx_stride,
		double *m_outputy, size_t m_outputy_stride)
{
	size_t i;
	switch (m_kernel)
	{
		case kernel_type_epanechnikov:
		{
			for (i = 0; i < m_numoutput; i++)
			{
				m_outputy[i * m_outputy_stride] = ebex_stats_convolve_epanechnikov(m_inputy, m_inputy_stride, m_inputx, m_inputx_stride, m_bandwidth, m_outputx[i * m_outputx_stride], m_numinput);
			}
			break;
		}
		case kernel_type_uniform:
		{
			for (i = 0; i < m_numoutput; i++)
			{
				m_outputy[i * m_outputy_stride] = ebex_stats_convolve_uniform(m_inputy, m_inputy_stride, m_inputx, m_inputx_stride, m_bandwidth, m_outputx[i * m_outputx_stride], m_numinput);
			}
			break;
		}
		case kernel_type_triangle:
		{
			for (i = 0; i < m_numoutput; i++)
			{
				m_outputy[i * m_outputy_stride] = ebex_stats_convolve_triangle(m_inputy, m_inputy_stride, m_inputx, m_inputx_stride, m_bandwidth, m_outputx[i * m_outputx_stride], m_numinput);
			}
			break;
		}
		case kernel_type_gaussian:
		{
			for (i = 0; i < m_numoutput; i++)
			{
				m_outputy[i * m_outputy_stride] = ebex_stats_convolve_gaussian(m_inputy, m_inputy_stride, m_inputx, m_inputx_stride, m_bandwidth, m_outputx[i * m_outputx_stride], m_numinput);
			}
			break;
		}
		case kernel_type_quartic:
		{
			for (i = 0; i < m_numoutput; i++)
			{
				m_outputy[i * m_outputy_stride] = ebex_stats_convolve_quartic(m_inputy, m_inputy_stride, m_inputx, m_inputx_stride, m_bandwidth, m_outputx[i * m_outputx_stride], m_numinput);
			}
			break;
		}
		case kernel_type_triweight:
		{
			for (i = 0; i < m_numoutput; i++)
			{
				m_outputy[i * m_outputy_stride] = ebex_stats_convolve_triweight(m_inputy, m_inputy_stride, m_inputx, m_inputx_stride, m_bandwidth, m_outputx[i * m_outputx_stride], m_numinput);
			}
			break;
		}
		case kernel_type_cosine:
		{
			for (i = 0; i < m_numoutput; i++)
			{
				m_outputy[i * m_outputy_stride] = ebex_stats_convolve_cosine(m_inputy, m_inputy_stride, m_inputx, m_inputx_stride, m_bandwidth, m_outputx[i * m_outputx_stride], m_numinput);
			}
			break;
		}
		case kernel_type_tricubic:
		{
			for (i = 0; i < m_numoutput; i++)
			{
				m_outputy[i * m_outputy_stride] = ebex_stats_convolve_tricubic(m_inputy, m_inputy_stride, m_inputx, m_inputx_stride, m_bandwidth, m_outputx[i * m_outputx_stride], m_numinput);
			}
			break;
		}
	}
}

double ebex_stats_bandwidth(int m_numdata, double *m_data,
		size_t m_datastride, double *m_weight, size_t m_weightstride, e_kernel_type m_kernel)
{
	int i;
	double mean = 0.0;
	double std = 0.0;
	double nd = 0.0;
	double retval;
	if (m_weight)
	{
		for (i = 0; i < m_numdata; i++)
		{
			const double x = m_data[i * m_datastride];
			const double w = m_weight[i * m_weightstride];
			mean += x * w;
			std += x * x * w;
			nd += w;
		}
	}
	else
	{
		for (i = 0; i < m_numdata; i++)
		{
			const double x = m_data[i * m_datastride];
			mean += x;
			std += x * x;
		}
		nd = m_numdata;
	}
	if (nd <= 1.0)
	{
		return -1.0; /* Not enough m_data */
	}
	std -= mean * mean / nd;
	std = sqrt(std / (nd - 1)); /* standard deviation */
	switch (m_kernel)
	{
		case kernel_type_epanechnikov:
			retval = std * pow(80.0 / (M_2_SQRTPI * nd), 0.2);
			break;
		case kernel_type_uniform:
			retval = std * pow(24.0 / (M_2_SQRTPI * nd), 0.2);
			break;
		case kernel_type_triangle:
			retval = std * pow(128.0 / (M_2_SQRTPI * nd), 0.2);
			break;
		case kernel_type_gaussian:
			retval = std * pow(4.0 / (3.0 * nd), 0.2);
			break;
		case kernel_type_quartic:
			retval = std * pow(560.0 / (3.0 * M_2_SQRTPI * nd), 0.2);
			break;
		case kernel_type_triweight:
			retval = std * pow(50400.0 / (143.0 * M_2_SQRTPI * nd), 0.2);
			break;
		case kernel_type_cosine:
		{
			const double k = M_PI * M_PI - 8;
			retval = std * pow(M_PI, 0.65) / pow(6.0 * k * k * nd, 0.2);
			break;
		}
		default:
			retval = nan("Bad Type");
			die("Unknown kernel type");
			break;
	}
	return retval;
}


static int
ebex_stats_local_weighted_est(const double *m_inputy, const double *m_inputx, size_t m_numinput, double m_xval, double *m_outputy, long nleft, long nright,
		double *m_residual, double *m_residualweight)
{
	double range;
	double bandwidth;
	double lower_bandwidth, upper_bandwidth;
	double total_residual, b, c, r;
	long j, upper_index_limit;

	range = m_inputx[m_numinput - 1] - m_inputx[0];
	bandwidth = fmax(m_xval - m_inputx[nleft], m_inputx[nright] - m_xval);
	upper_bandwidth = 0.999 * bandwidth;
	lower_bandwidth = 0.001 * bandwidth;

	if (!m_numinput)
	{
		elog(log_level_warning, "Passed a zero-sized vector");
	}

	/// Compute weights (ties go to the end)
	total_residual = 0.0;
	for (j = nleft; j < m_numinput; j++)
	{
		m_residual[j] = 0.0;
		r = fabs(m_inputx[j] - m_xval);
		if (r <= upper_bandwidth)
		{ /* small enough for non-zero weight */
			if (r > lower_bandwidth)
				m_residual[j] = pow(1.0 - pow(r / bandwidth, 3.0), 3.0);
			else
				m_residual[j] = 1.0;
			if (m_residualweight) m_residual[j] = m_residualweight[j] * m_residual[j];
			total_residual += m_residual[j];
		}
		else if (m_inputx[j] > m_xval) break; /* get out at first zero wt on right */
	}
	upper_index_limit = j - 1;
	if (total_residual > 0.0)
	{
		/// Normalize the residuals
		for (j = nleft; j <= upper_index_limit; j++)
			m_residual[j] = m_residual[j] / total_residual;

		/// Linear fitting
		if (bandwidth > 0.0)
		{
			total_residual = 0.0;
			c = 0.0;

			/// Find weighted mean of input
			for (j = nleft; j <= upper_index_limit; j++)
				total_residual += m_residual[j] * m_inputx[j];

			b = m_xval - total_residual;
			for (j = nleft; j <= upper_index_limit; j++)
				c += m_residual[j] * (m_inputx[j] - total_residual) * (m_inputx[j] - total_residual);

			if (sqrt(c) > .001 * range)
			{
				/* points are spread out enough to compute slope */
				b = b / c;
				for (j = nleft; j <= upper_index_limit; j++)
					m_residual[j] = m_residual[j] * (1.0 + b * (m_inputx[j] - total_residual));
			}
		}

		*m_outputy = 0.0;
		for (j = nleft; j <= upper_index_limit; j++)
			*m_outputy += m_residual[j] * m_inputy[j];

		return EBEX_SUCCESS;
	}

	return EBEX_FAIL;
}

static inline void ebex_stats_robust_weight(double *m_robust_weight, const double *m_residual, size_t m_numinput)
{
	size_t m1;
	size_t m2;
	size_t i;
	double cmad;
	double c1, c9;
	double r;

	if (!m_robust_weight || !m_residual) return;

	for (i = 0; i < m_numinput; i++)
		m_robust_weight[i] = fabs(m_residual[i]);

	sort_double(m_robust_weight, m_numinput);
	m1 = 1 + m_numinput / 2;
	m2 = m_numinput - m1 + 1;
	cmad = 3.0 * (m_robust_weight[m1] + m_robust_weight[m2]); /* 6 median abs resid */
	c9 = .999 * cmad;
	c1 = .001 * cmad;
	for (i = 0; i < m_numinput; i++)
	{
		r = fabs(m_residual[i]);
		if (r <= c1)
			m_robust_weight[i] = 1.0; /* near 0, avoid underflow */
		else if (r > c9)
			m_robust_weight[i] = 0.0; /* near 1, avoid underflow */
		else
			m_robust_weight[i] = pow(1.0 - pow(r / cmad, 2.0), 2.0);
	}
}

int
ebex_stats_lowess(const double *m_inputx, const double *m_inputy, size_t m_numinput,
		size_t m_windowsize,
		size_t m_numiter,
		double m_delta,
		double *m_outputy, size_t m_outputy_stride,
		double *m_robust_weight, double *m_residual)
{
	long i, j, last;
	size_t nleft, nright;
	double cut;
	double *resid_weight = NULL;

	if (m_numinput < 2)
	{
		m_outputy[0] = m_inputy[0];
		elog(log_level_warning, "Bad input number to local weighted estimation");
		return 1;
	}

	m_windowsize = MAX(MIN(m_windowsize, m_numinput), 2); /* at least two, at most n points */
	for (size_t iter = 0; iter < m_numiter; iter++)
	{ /* robustness iterations */
		nleft = 0;
		nright = m_windowsize - 1;
		last = -1; /* index of prev estimated point */
		i = 0; /* index of current point */
		if (iter > 0) resid_weight = m_robust_weight;

		while (last < (int) (m_numinput - 1))
		{
			while (nright < m_numinput - 1 && (m_inputx[i] - m_inputx[nleft] > m_inputx[nright + 1] - m_inputx[i]))
			{
				/// If the radius decreases, move nleft,nright to the right
				nleft++;
				nright++;
			}
			if (ebex_stats_local_weighted_est(m_inputy, m_inputx,
					m_numinput, m_inputx[i],
					&m_outputy[i * m_outputy_stride],
					nleft, nright,
					m_residual, resid_weight) != EBEX_SUCCESS)
			{
				m_outputy[i * m_outputy_stride] = m_inputy[i];
			}

			/// all weights zero - copy over value
			if (last < i - 1)
			{
				/// Interpolate over skipped points
				double denom;
				double alpha;
				denom = m_inputx[i] - m_inputx[last];
				for (j = last + 1; j < i; j = j + 1)
				{
					alpha = (m_inputx[j] - m_inputx[last]) / denom;
					m_outputy[j * m_outputy_stride] = alpha * m_outputy[i * m_outputy_stride] + (1.0 - alpha) * m_outputy[last * m_outputy_stride];
				}
			}
			last = i; /* last point actually estimated */
			cut = m_inputx[last] + m_delta; /* x coord of close points */
			for (i = last + 1; i < m_numinput && m_inputx[i] <= cut; i++)
			{ /* find close points */
				if (m_inputx[i] == m_inputx[last])
				{ /* exact match in x */
					m_outputy[i * m_outputy_stride] = m_outputy[last * m_outputy_stride];
					last = i;
				}
			}
			i = MAX(last + 1, i - 1);
			/* back 1 point so interpolation within delta, but always go forward */
		}
		for (i = 0; i < m_numinput; i++) /* residuals */
		{
			m_residual[i] = m_inputy[i] - m_outputy[i * m_outputy_stride];
		}

		if (iter < m_numiter)
		{
			ebex_stats_robust_weight(m_robust_weight, m_residual, m_numinput);
		}
	}
	return EBEX_SUCCESS;
}


